#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include "processing.h"
#include "../device/device.h"

using namespace std;
using namespace cv;

int labelMatrix[BOARD_HEIGHT][BOARD_WIDTH];
cv::Size BOARD_SIZE(BOARD_WIDTH, BOARD_HEIGHT);
cv::Point2f srcAnchors[4];
cv::Point2f dstCorners[4] = {
        cv::Point(20, 15),
        cv::Point(880, 15),
        cv::Point(880, 535),
        cv::Point(20, 535)
};
uint selectedAnchorNum = 0;
string winname = "Select anchors";
cv::Mat transformMatrix;
bool state;
static cv::Mat frame;
char* anchorPath;

bool loadLabelMatrix(const char* path) {
    try {
        ifstream file(path);
        if (!file.is_open()) throw string("labelMatrix is not open!");
        for (int i = 0; i < BOARD_HEIGHT; ++i) {
            for (int j = 0; j < BOARD_WIDTH; ++j) {
                file >> labelMatrix[i][j];
            }
        }
        file.close();
        return true;
    }
    catch (...) {
        cerr << "labelMatrix load failed!" << endl;
        return false;
    }
}

int getLabel(cv::Rect& bbox) {
    int x = bbox.x + bbox.width / 2;
    int y = bbox.y + bbox.height / 2;
    return labelMatrix[y][x];
}

int getLabel(uint x, uint y) {
    return labelMatrix[y][x];
}

void postprocessing() {}

inline void drawCross(cv::Mat& src, cv::Point center, cv::Scalar color = GREEN, int radius = 20, int thickness = 2) {
    static int x1, y1, x2, y2;
    x1 = center.x - radius, y1 = center.y;
    x2 = center.x + radius, y2 = center.y;
    cv::line(src, Point(x1, y1), Point(x2, y2), color, thickness, 8, 0);
    x1 = center.x, y1 = center.y - radius;
    x2 = center.x, y2 = center.y + radius;
    cv::line(src, Point(x1, y1), Point(x2, y2), color, thickness, 8, 0);
}

void display(cv::Mat& img) {
    for (uint i = 0; i < selectedAnchorNum; ++i) {
        drawCross(img, srcAnchors[i]);
    }
    cv::putText(img, "Select anchors in turn: ", Point(30, 30), 0, 0.8, WHITE, 1, 8);
    cv::putText(img, "top left -> top right -> bottom right -> bottom left", Point(60, 70), 0, 0.8, WHITE, 1, 8);
    cv::putText(img, "Left  click: select", Point(30, 110), 0, 0.8, WHITE, 1, 8);
    cv::putText(img, "Space press: next", Point(30, 150), 0, 0.8, WHITE, 1, 8);
}

cv::Point2i getCrossPoint(vector<int>& lineA, vector<int>& lineB) {
    cv::Point2i crossPt;
    auto k1 = ((double)lineA[3] - lineA[1]) / ((double)lineA[2] - lineA[0] + 1e-4);
    auto k2 = ((double)lineB[3] - lineB[1]) / ((double)lineB[2] - lineB[0] + 1e-4);
    k1 = abs(k1) <= 0.2 ? 0 : k1;
    k2 = abs(k2) <= 0.2 ? 0 : k2;
    auto b1 = lineA[1] - k1 * lineA[0];
    auto b2 = lineB[1] - k2 * lineB[0];
    if (cvRound(abs(k1)) >= 10) {
        crossPt.x = lineA[0];
        crossPt.y = cvRound(k2 * crossPt.x + b2);
    }
    else if (cvRound(abs(k2)) >= 10) {
        crossPt.x = lineB[0];
        crossPt.y = cvRound(k1 * crossPt.x + b1);
    }
    else {
        crossPt.x = cvRound(-(b1 - b2) / (k1 - k2));
        crossPt.y = cvRound(k1 * crossPt.x + b1);
    }
    return crossPt;
}

void onMouse(int event, int x, int y, int flag, void* userdata) {
    cv::Mat& img = *(cv::Mat*) userdata;
    switch (event) {
    case cv::EVENT_MOUSEMOVE:       // move
        break;

    case cv::EVENT_LBUTTONDOWN:     // left -> select
        printf("click (%d, %d)\n", x, y);
        if (selectedAnchorNum < 4) {
            srcAnchors[selectedAnchorNum].x = (float)x;
            srcAnchors[selectedAnchorNum].y = (float)y;
            selectedAnchorNum++;
        }
        break;

    case cv::EVENT_RBUTTONDOWN:     // right -> delete
        if (selectedAnchorNum > 0) selectedAnchorNum--;
    }
}

void locate(const cv::Mat& src, int cx, int cy, int d) {
    int x1 = cx - d / 2, y1 = cy - d / 2, w = d, h = d;
    cv::Rect bbox(x1, y1, w, h);
    cv::Mat roi, dst, SE;
    src(bbox).copyTo(roi);
    cv::cvtColor(roi, roi, cv::COLOR_BGR2GRAY);
    cv::GaussianBlur(roi, roi, cv::Size(5, 5), 2.5);
    cv::threshold(roi, roi, 0, 255, cv::THRESH_OTSU + cv::THRESH_BINARY);

    dst = roi.clone();
    SE = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(1, d / 5));
    cv::morphologyEx(dst, dst, cv::MORPH_CLOSE, SE);
    vector<int> lineA;
    for (int i = 0; i < h; i += h / 5) {
        int j1 = 0, j2 = 0;
        for (int j = 1; j < w - 1; j++) {
            if ((int)dst.at<uchar>(i, j) == 0 && (int)dst.at<uchar>(i, j - 1) == 255) {
                j1 = j;
            }
            if ((int)dst.at<uchar>(i, j) == 0 && (int)dst.at<uchar>(i, j + 1) == 255) {
                j2 = j;
                break;
            }
        }
        if (j1 > 0 && j2 > 0) {
            lineA.push_back(x1 + (j1 + j2) / 2);
            lineA.push_back(y1 + i);
            if (lineA.size() == 4) break;
        }
    }
    assert(lineA.size() == 4);
    //printf("(%d,%d)  (%d,%d)\n", lineA[0], lineA[1], lineA[2], lineA[3]);

    dst = roi.clone();
    SE = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(d / 4, 1));
    cv::morphologyEx(dst, dst, cv::MORPH_CLOSE, SE);
    vector<int> lineB;
    for (int j = 0; j < w; j += w / 5) {
        int i1 = 0, i2 = 0;
        for (int i = 1; i < h - 1; i++) {
            if ((int)dst.at<uchar>(i, j) == 0 && (int)dst.at<uchar>(i - 1, j) == 255) {
                i1 = i;
            }
            if ((int)dst.at<uchar>(i, j) == 0 && (int)dst.at<uchar>(i + 1, j) == 255) {
                i2 = i;
                break;
            }
        }
        if (i1 > 0 && i2 > 0) {
            lineB.push_back(x1 + j);
            lineB.push_back(y1 + (i1 + i2) / 2);
            if (lineB.size() == 4) break;
        }
    }
    assert(lineB.size() == 4);
    //printf("(%d,%d)  (%d,%d)\n", lineB[0], lineB[1], lineB[2], lineB[3]);

    cv::Point pt = getCrossPoint(lineA, lineB);
    cx = pt.x, cy = pt.y;
}


bool getTransformMatrix() {
    try {
        transformMatrix = cv::getPerspectiveTransform(srcAnchors, dstCorners);
        return true;
    }
    catch (...) {
        return false;
    }
}

void loadAnchors() {
    printf("Anchor loading ... ");
    try {
        ifstream file(anchorPath);
        if (!file.is_open()) throw string("anchor.txt is not open!");
        printf("Done.\n");
        for (int i = 0; i < 4; ++i) {
            file >> srcAnchors[i].x;
            file >> srcAnchors[i].y;
            printf("  (%d, %d)\n", (int)srcAnchors[i].x, (int)srcAnchors[i].y);
        }
        file.close();
        selectedAnchorNum = 4;
    }
    catch (...) {
        selectedAnchorNum = 0;
        printf("ERROR.\n");
    }
}

void saveAnchors() {
    ofstream file(anchorPath);
    for (int i = 0; i < 4; ++i) {
        file << (int)srcAnchors[i].x << " " << (int)srcAnchors[i].y << endl;
    }
    file.close();
}


void selectAnchors() {
    readFrame(frame);
    cv::namedWindow(winname);
    cv::setMouseCallback(winname, onMouse, &frame);
    while (true) {
        readFrame(frame);
        display(frame);
        cv::imshow(winname, frame);
        int key = cv::waitKey(30);
        key = 13; // Debug
        if (key == 13 && selectedAnchorNum == 4) { // Enter
            state = getTransformMatrix();
            if (state) {
                saveAnchors();
                printf("Calibration successful\n");
                break;
            }
            else {
                selectedAnchorNum = 0;
            }
        }
        else if (key == int('z') || key == int('Z')) {
            selectedAnchorNum = 0;
        }
        else if (key == 27) { // ESC
            break;
        }
        else if (key == 32) { // space
            break;
        }
        else continue;
        display(frame);
        cv::imshow(winname, frame);
    }
    cv::destroyAllWindows();
}

void perspectiveTransform(cv::Mat& img) {
    cv::warpPerspective(img, img, transformMatrix, BOARD_SIZE);
}

cv::Point2i getCrossPoint(cv::Vec4i& lineA, cv::Vec4i& lineB) {
    cv::Point2i crossPt;
    auto k1 = ((double)lineA[3] - lineA[1]) / ((double)lineA[2] - lineA[0] + 1e-4);
    auto k2 = ((double)lineB[3] - lineB[1]) / ((double)lineB[2] - lineB[0] + 1e-4);
    auto b1 = lineA[1] - k1 * lineA[0];
    auto b2 = lineB[1] - k2 * lineB[0];
    if (abs(k1) > 1e4) {
        crossPt.x = lineA[0];
        crossPt.y = cvRound(k2 * crossPt.x + b2);
    }
    else if (abs(k2) > 1e4) {
        crossPt.x = lineB[0];
        crossPt.y = cvRound(k1 * crossPt.x + b1);
    }
    else {
        crossPt.x = cvRound(-(b1 - b2) / (k1 - k2));
        crossPt.y = cvRound(k1 * crossPt.x + b1);
    }
    return crossPt;
}


void calibrate(const char* path) {
    anchorPath = (char*)path;
    loadAnchors();
    selectAnchors();
    getTransformMatrix();
}